// /////////////////////////////////////////////////////////////////////////////
//
// Name:            AICore.cpp
// Author:          Michael Bartsch
//
// Desc :           This file contains the needed classes for A* pathfinding.
//                  A* pathfinding searches for the quickest path between a
//                  begin point and the target point.
//
// License:         Copyright (C) 2008-2009 Michael Bartsch
//
//                  This program is free software: you can redistribute it
//                  and/or modify it under the terms of the GNU General Public
//                  License as published by the Free Software Foundation,
//                  either version 3 of the License, or (at your option) any
//                  later version.
//
//                  This program is distributed in the hope that it will be
//                  useful, but WITHOUT ANY WARRANTY; without even the implied
//                  warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
//                  PURPOSE. See the GNU General Public License for more
//                  details.
//
//                  You should have received a copy of the GNU General Public
//                  License along with this program. If not, see
//                  <http://www.gnu.org/licenses/>.
//
// /////////////////////////////////////////////////////////////////////////////

// Include required header file(s).
#include "AICore.h"


// CAStarNode
// Constructor
CAStarNode::CAStarNode()
: parent(NULL), x(0), y(0), state(ASN_Unknown), onOpenList(false), onClosedList(false),
G(0), F(0), H(0)
{
}

// Deconstructor
CAStarNode::~CAStarNode()
{
}


// Sorting function.
// Sorts a list based upon the F value of a node.
bool sortListF(CAStarNode *i, CAStarNode *j)
{
    return (i->F < j->F);
}


// CAICore
// Constructor
CAICore::CAICore()
: isPathFound(false), startX(50), startY(50), targetX(50), targetY(49), isTarget(false),
  agentState(0), rotation(0), isWaiting(false)
{
   nodeMap = new CAStarNode[100*100];

   for(int y = 0; y < 100; y++)
   {
      for(int x = 0; x < 100; x++)
      {
         nodeMap[y*100+x].x = x;
         nodeMap[y*100+x].y = y;
      }
   }
}

// Deconstructor
CAICore::~CAICore()
{
    delete [] nodeMap;
}

// CCommunicator singleton.
CAICore* CAICore::pInstance = 0;

CAICore* CAICore::Instance()
{
	if(pInstance == 0)
	  pInstance = new CAICore;

	return pInstance;
}

// Updates the AI Core.
void CAICore::update()
{
   // Is the agent waiting for the sonar?
   if(!isWaiting && agentState != 0)
   {
      // Check if the direct environment is known.
      checkEnvironment();

      // Calculate the new path to the destination and get the next step.
      if(calculatePath() == ASP_Found)
      {
         // Forward declarations.
         int rotate = 0;
         char serialBuffer[64];

         // Determine where to move to.
         int x = nextNode->x-startX;
         int y = nextNode->y-startY;

         startX = nextNode->x;
         startY = nextNode->y;

         // Determine the rotation for the x-direction.
         memset(serialBuffer, 0, 64);
         rotate = -1; // Special case

         if(x == -1)
         {
            rotate = rotation+90;
            rotation = -90;
         }

         else if(x == 1)
         {
            rotate = rotation-90;
            rotation = 90;
         }

         // Execute the rotation and drive into the direction.
         if(rotate == -1)
           sprintf(serialBuffer, "nothing");

         else if(rotate < 0)
           sprintf(serialBuffer, "<in rotate -r %d><in motor -f 25>", abs(rotate));

         else if(rotate > 0)
           sprintf(serialBuffer, "<in rotate -l %d><in motor -f 25>", rotate);

         else sprintf(serialBuffer, "<in motor -f 25>");

         CCommunicator::Instance()->serialWrite(serialBuffer);

         // Determine the rotation for the y-direction.
         memset(serialBuffer, 0, 64);
         rotate = -1; // Special case

         if(y == -1)
         {
            rotate = rotation;
            rotation = 0;
         }

         else if(y == 1)
         {
            rotate = rotation-180;
            rotation = 180;
         }

         // Execute the rotation and drive into the direction.
         if(rotate == -1)
           sprintf(serialBuffer, "<out done>");

         else if(rotate < 0)
           sprintf(serialBuffer, "<in rotate -r %d><in motor -f 25><out done>", abs(rotate));

         else if(rotate > 0)
           sprintf(serialBuffer, "<in rotate -l %d><in motor -f 25><out done>", rotate);

         else sprintf(serialBuffer, "<in motor -f 25><out done>");

         isWaiting = true;
         CCommunicator::Instance()->serialWrite(serialBuffer);
      }
   }
}

// Calculates a path to the target location using the A* algorithm.
int CAICore::calculatePath()
{
    // 1. Check for quick paths: Is a path really needed?
    // - Do we have a target location?
    if(!isTarget)
      return ASP_NoPath;

    // - Is the start location the target location?
    if(startX == targetX && startY == targetY)
      return ASP_Found;

    // - Is the target location walkable?
    if(nodeMap[targetY*100+targetX].state == ASN_Unwalkable)
      return ASP_NoPath;

    // - Do we have match with a possible colour target?
    if(CColourAnalyser::Instance()->getIsTarget() && CColourAnalyser::Instance()->getIsMatchRGB())
      return ASP_Found;

    // 2. Reset variables.
    openList.clear();

    for(int i = 0; i < 100; i++)
    {
       for(int j = 0; j < 100; j++)
       {
          nodeMap[i*100+j].onOpenList = false;
          nodeMap[i*100+j].onClosedList = false;
       }
    }

    pathLength = 0;

    // 3. Add start node to the open list.
    currentNode = &nodeMap[startY*100+startX];
    openList.push_back(currentNode);
    currentNode->onOpenList = true;

    // 4. Find path...
    while(currentNode->x != targetX || currentNode->y != targetY)
    {
        // - Are there any nodes on the open list?
        if(openList.size() > 0)
        {
           // 4.a. Order the list.
           sort(openList.begin(), openList.end(), sortListF);

           // 4.b. Get the best node, remove it from the open list and add it to the closed list.
           currentNode = openList[0];
           openList.erase(openList.begin());

           currentNode->onClosedList = true;
           currentNode->onOpenList = false;

           // 4.c. Check adjecent nodes.
           for(int y = currentNode->y-1; y <= currentNode->y+1; y++)
           {
              for(int x = currentNode->x-1; x <= currentNode->x+1; x++)
              {
                  // - Not out of bounds?
                  if(x >= 0 && y >= 0 && x < 100 && y < 100)
                  {

                  // - Get pointer to the selected node.
                  CAStarNode *selectedNode = &nodeMap[y*100+x];

                  // - On closed list?
                  if(!selectedNode->onClosedList)
                  {

                  // - Walkable?
                  if(selectedNode->state != ASN_Unwalkable)
                  {

                  // - Is the corner walkable?
                  bool isCornerWalkable = true;

                  if(x == currentNode->x-1)
                  {
                     if(y == currentNode->y-1)
                     {
                        if(nodeMap[currentNode->y*100+x-1].state == ASN_Unwalkable ||
                           nodeMap[(currentNode->y-1)*100+x].state == ASN_Unwalkable)
                        {
                           isCornerWalkable = false;
                        }
                     }

                     else if(y == currentNode->y+1)
                     {
                        if(nodeMap[currentNode->y*100+x-1].state == ASN_Unwalkable ||
                           nodeMap[(currentNode->y+1)*100+x].state == ASN_Unwalkable)
                        {
                           isCornerWalkable = false;
                        }
                     }
                  }

                  else if(x == currentNode->x+1)
                  {
                     if(y == currentNode->y-1)
                     {
                        if(nodeMap[currentNode->y*100+x+1].state == ASN_Unwalkable ||
                           nodeMap[(currentNode->y-1)*100+x].state == ASN_Unwalkable)
                        {
                           isCornerWalkable = false;
                        }
                     }

                     else if(y == currentNode->y+1)
                     {
                        if(nodeMap[currentNode->y*100+x+1].state == ASN_Unwalkable ||
                           nodeMap[(currentNode->y+1)*100+x].state == ASN_Unwalkable)
                        {
                           isCornerWalkable = false;
                        }
                     }
                  }

                  if(isCornerWalkable)
                  {

                  // - Not on the open list yet?
                  if(!selectedNode->onOpenList)
                  {
                      // -- Determine G.
                      int G;

                      if(abs(x-currentNode->x) == 1 && abs(y-currentNode->y) == 1)
                        G = 14; // Diagonal movement

                      else
                        G = 10; // Non-diagonal movement

                      G += currentNode->G;

                      // Add movement cost based upon agent state and node state.
                      if(agentState == 1) // In case more states are added.
                      {
                         switch(currentNode->state)
                         {
                            case ASN_Walkable: G += 0;  break;
                            case ASN_Unknown:  G += 20; break;
                         }
                      }

                      // -- Determine H.
                      int H = 10*(abs(x-targetX)+abs(y-targetY));

                      // -- Determine F.
                      int F = G + H;

                      // -- Alter node.
                      selectedNode->parent = currentNode;
                      selectedNode->F = F;
                      selectedNode->G = G;
                      selectedNode->H = H;
                      selectedNode->onOpenList = true;

                      // -- Add node to the open list.
                      openList.push_back(selectedNode);
                  } // if(!selectedNode->onOpenList)

                  // - On the open list?
                  else
                  {
                      // -- Determine G.
                      int G;

                      if(abs(x-currentNode->x) == 1 && abs(y-currentNode->y) == 1)
                        G = 14; // Diagonal movement

                      else
                        G = 10; // Non-diagonal movement

                      G += currentNode->G;

                      // Add movement cost based upon agent state and node state.
                      if(agentState == 1) // In case we add more states.
                      {
                         switch(currentNode->state)
                         {
                            case ASN_Walkable: G += 0;  break;
                            case ASN_Unknown:  G += 20; break;
                         }
                      }

                      // -- Is the new G smaller than previous G values?
                      if(G < selectedNode->G)
                      {
                          // -- Determine H and F.
                          int H = selectedNode->H;
                          int F = G + H;

                          // -- Alter node.
                          selectedNode->parent = currentNode;
                          selectedNode->F = F;
                          selectedNode->G = G;

                          // -- Add node to the open list.
                          openList.push_back(selectedNode);
                      }
                  } // if(!selectedNode->onOpenList) || else if(selectedNode->onOpenList)
                  } // if(isCornerWalkable)
                  } // if(selectedNode->state != ASN_Unwalkable)
                  } // if(!selectedNode->onClosedList)
                  } // if(x >= 0 && y >= 0 && x < 100 && y < 100)
              } // for(int x = currentNode->x-1; x < currentNode->x+-; x++)
           } // for(int y = currentNode->y-1; y < currentNode->y+1; y++)
        } // if(openList.size() > 0)

        // - There is no path.
        else
        {
            return ASP_NoPath;
        }

        // - Has a path been found?
        if(currentNode->x == targetX && currentNode->y == targetY)
        {
            isPathFound = true;
        }

    } // while(currentNode->x != targetX || currentNode->y != targetY)

    // 5. Retrieve the path.
    if(isPathFound)
    {
        CAStarNode *selectedNode = &nodeMap[targetY*100+targetX];
        pathList.clear();

        while(selectedNode->parent != &nodeMap[startY*100+startX])
        {
            pathList.insert(pathList.begin(), selectedNode);
            selectedNode = selectedNode->parent;
        }

        nextNode = selectedNode;
    }

    return ASP_Found;
}

// Checks the environment for possible new data.
void CAICore::checkEnvironment()
{
   // Check cells that have a distance of approximately 1.
   bool isEnvKnown = true;

   for(int y = startY-1; y < startY+1; y++)
   {
      for(int x = startX-1; x < startX+1; x++)
      {
         // Are we within the boundaries?
         if(x >= 0 && y >= 0 && x < 100 && y < 100)
         {

         // Check if the selected cell is known.
         if(nodeMap[y*100+x].state == ASN_Unknown)
         {
            // Set a preliminary result.
            isEnvKnown = false;

            // Are the corners actually walkable?
            if(x == startX-1)
            {
               if(y == startY-1)
               {
                  if(nodeMap[startY*100+startX-1].state == ASN_Unwalkable ||
                     nodeMap[(startY-1)*100+startX].state == ASN_Unwalkable)
                  {
                     isEnvKnown = true;
                  }
               }

               else if(y == startY+1)
               {
                  if(nodeMap[startY*100+startX-1].state == ASN_Unwalkable ||
                     nodeMap[(startY+1)*100+startX].state == ASN_Unwalkable)
                  {
                     isEnvKnown = true;
                  }
               }
            }

            else if(x == startX+1)
            {
               if(y == startY-1)
               {
                  if(nodeMap[startY*100+startX+1].state == ASN_Unwalkable ||
                     nodeMap[(startY-1)*100+startX].state == ASN_Unwalkable)
                  {
                     isEnvKnown = true;
                  }
               }

               else if(y == startY+1)
               {
                  if(nodeMap[startY*100+startX+1].state == ASN_Unwalkable ||
                     nodeMap[(startY+1)*100+startX].state == ASN_Unwalkable)
                  {
                     isEnvKnown = true;
                  }
               }
            }

            // Is the environment still known?
            if(!isEnvKnown)
              break;

         } // if(nodeMap[y*100+x].state == ASN_Unknown)
         } // if(x >= 0 && y >= 0 && x < 100 && y < 100)
      } // for(int x = startX-1; x < startX+1; x++)
   } // for(int y = startY-1; y < startY+1; y++)

   // Is the environment known?
   if(!isEnvKnown)
   {
      // Rotate the Aia to check the environment.
      if(rotation-90 == -180)
	    rotation = 180;

	  else rotation -= 90;

	  isWaiting = true;
      CCommunicator::Instance()->serialWrite("<in rotate -l 90><out sonar><out done>");
   }
}

// Get Functions - [ BEGIN ]
// Returns the pointer to the given node.
CAStarNode* CAICore::getNode(int x, int y)
{
   return &nodeMap[y*100+x];
}

// Returns the x-coordinate of the starting position.
int CAICore::getStartX()
{
   return startX;
}

// Returns the y-coordinate of the starting position.
int CAICore::getStartY()
{
   return startY;
}

// Returns the x-coordinate of the target position.
int CAICore::getTargetX()
{
   return targetX;
}

// Returns the y-coordinate of the target position.
int CAICore::getTargetY()
{
   return targetY;
}

// Returns the x-coordinate of the next cell.
int CAICore::getNextX()
{
   return nextNode->x;
}

// Returns the Y-coordinate of the next cell.
int CAICore::getNextY()
{
   return nextNode->y;
}

// Returns the virtual rotation of the agent.
int CAICore::getRotation()
{
   return rotation;
}

// Does the agent have a target?
bool CAICore::getIsTarget()
{
   return isTarget;
}

// Gets the state of the agent.
int CAICore::getAgentState()
{
   return agentState;
}

// Gets the waiting state of the AI Core.
bool CAICore::getIsWaiting()
{
   return isWaiting;
}
// Get Functions - [ END ]

// Set Functions - [ BEGIN ]
// Sets the start location.
void CAICore::setStartLocation(int x, int y)
{
   startX = x;
   startY = y;
}

// Sets the x-coordinate of the start location.
void CAICore::setStartX(int x)
{
   startX = x;
}

// Sets the y-coordinate of the start location.
void CAICore::setStartY(int y)
{
   startY = y;
}

// Sets the target location.
void CAICore::setTargetLocation(int x, int y)
{
   targetX = x;
   targetY = y;
}

// Sets the x-coordinate of the target location.
void CAICore::setTargetX(int x)
{
   targetX = x;
}

// Sets the y-coordinate of the target location.
void CAICore::setTargetY(int y)
{
   targetY = y;
}

// Sets the virtual rotation of the agent.
void CAICore::setRotation(int value)
{
   if(value >= -180 && value <= 180)
     rotation = value;
}

// Sets the state of the selected node.
void CAICore::setNodeState(int x, int y, AStarNodeStates state)
{
   if(x >= 0 && x < 100 && y >= 0 && y < 100)
     nodeMap[y*100+x].state = state;
}

// Sets the states of all nodes up to the given distance.
void CAICore::setNodesSonar(int distance)
{
   // Determine direction.
   int i;

   switch(rotation)
   {
      case 0: // North

         for(i = startY; i > startY-(distance/20)-1; i--)
         {
            if(i >= 0 && i < 100)
              setNodeState(startX, i, ASN_Walkable);
         }

         if(i >= 0 && i < 100)
           setNodeState(startX, i, ASN_Unwalkable);

         break;

      case 90: // East

         for(i = startX; i < startX+(distance/20)+1; i++)
         {
            if(i >= 0 && i < 100)
              setNodeState(i, startY, ASN_Walkable);
         }

         if(i >= 0 && i < 100)
          setNodeState(i, startY, ASN_Unwalkable);

         break;

      case 180: // South
      case -180:

         for(i = startY; i < startY+(distance/20)+1; i++)
         {
            if(i >= 0 && i < 100)
              setNodeState(startX, i, ASN_Walkable);
         }

         if(i >= 0 && i < 100)
           setNodeState(startX, i, ASN_Unwalkable);

         break;

      case -90: // West

         for(i = startX; i > startX-(distance/20)-1; i--)
         {
            if(i >= 0 && i < 100)
              setNodeState(i, startY, ASN_Walkable);
         }

         if(i >= 0 && i < 100)
          setNodeState(i, startY, ASN_Unwalkable);

         break;
   }
}

// Activate the given target.
void CAICore::setIsTarget(bool value)
{
   isTarget = value;
}

// Sets the state of the agent.
void CAICore::setAgentState(int value)
{
   agentState = value;
}

// Sets the waiting state of the AI Core.
void CAICore::setIsWaiting(bool value)
{
   isWaiting = value;
}

// Resets the node map.
void CAICore::resetMap()
{
   delete [] nodeMap;

   nodeMap = new CAStarNode[100*100];

   for(int y = 0; y < 100; y++)
   {
      for(int x = 0; x < 100; x++)
      {
         nodeMap[y*100+x].x = x;
         nodeMap[y*100+x].y = y;
      }
   }
}
// Set Functions - [ END ]
// End of File


